#include "d3dImageModel.h"
#include <conio.h>
#include <fstream>



D3dImageModel::D3dImageModel()
{
	m_vNormal.x = 0.0f;
	m_vNormal.x = 0.0f;
	m_vNormal.y = -1.0f;

	m_vPosition.x = 0.0f;
	m_vPosition.y = 0.0f;
	m_vPosition.z = 5.0f;
}
D3dImageModel::~D3dImageModel()
{

}

void D3dImageModel::loafConfig(const string &fileName)
{
	ifstream ifile(fileName.c_str(), ios_base::in);
	ifile>>m_scene;  //read file

	m_mid    = cv::imread(m_scene.m_leftName, 1);
	m_depth  = cv::imread(m_scene.m_depthName, 0);
	m_normal = cv::imread(m_scene.m_normalName, 1);
	m_alpha  = cv::imread(m_scene.m_alphaName, 0);

	assert(!m_mid.empty());
	assert(!m_depth.empty());
	assert(!m_normal.empty());
	assert(!m_alpha.empty());
}

void D3dImageModel::load(string imageName, string depthMapName, string normalMapName, string alphaMapName)
{
	m_mid    = cv::imread(imageName, 1);
	m_depth  = cv::imread(depthMapName, 0);
	m_normal = cv::imread(normalMapName, 1);
	m_alpha  = cv::imread(alphaMapName, 0);

	assert(!m_mid.empty());
	assert(!m_depth.empty());
	assert(!m_normal.empty());
	assert(!m_alpha.empty());

	/*
	cv::resize(m_mid, m_mid, cv::Size(960, 540));
	cv::resize(m_depth, m_depth, cv::Size(960, 540));
	cv::resize(m_normal, m_normal, cv::Size(960, 540));
	cv::resize(m_alpha, m_alpha, cv::Size(960, 540));
	*/
}

void D3dImageModel::calcNormal(int x, int y, float &nx, float &ny, float &nz)
{
	if (x >=0 && x<m_normal.cols && y >= 0 && y<m_normal.rows)
	{
		cvx::Rgb8UImage nor_map(&m_normal);

		nx = nor_map[y][x].r/255.0;
		ny = nor_map[y][x].g/255.0;
		nz = nor_map[y][x].b/255.0;		

		nx = (nx - 0.5) * 2.0;
		ny = (ny - 0.5) * 2.0;
		nz = (nz - 0.5) * 2.0;

		
		nx =  -nx;
		ny =  -ny;
		nz =  nz;

		float PI = 3.14159;
		float theta = PI - acos(nz);
		float phi   = atan2(ny, nx) + PI;

		theta = theta/PI*180;
		phi   = phi/PI*180;

		_cprintf("theta and phi %f %f\n", theta, phi);	
		
		float len = sqrt(1.0 * (nx * nx + ny * ny + nz * nz));
		nx /= len;
		ny /= len;
		nz /= len;

		m_vNormal.x = nx;
		m_vNormal.y = ny;
		m_vNormal.z = nz;
	}
}


void D3dImageModel::calcPosition(int x, int y, float &px, float &py, float &pz)
{
	if(x >= 0 && x<m_depth.cols && y >= 0 && y<m_depth.rows)
	{
		assert(m_scene.m_depthResize != 0.0);
		int h = m_depth.rows;
		int w = m_depth.cols;	

		//reverse y
		y = h - y;

		cvx::Bw8UImage depth_map(&m_depth);
		px = x;
		py = y;
		pz = (depth_map[y][x]/255.0)/m_scene.m_depthResize - m_scene.m_depthOffset;

		

		float fc = m_scene.m_focalLength;
		float sw = m_scene.m_sensorWidth;
		float ratio = w/sw;
		fc *= ratio;

		//image the image plane in [0, 0, fc] and [-w/2, -h/2] to [w/2, h/2]
		nv::vec3f dir(x - w/2, y - h/2, fc);

		dir = nv::normalize(dir);
		assert(dir.z != 0.0);

		float scale = pz/dir.z;

		dir *= scale;

		px = dir.x;
		py = dir.y;
		pz = dir.z;

		m_vPosition = dir;

	}
}